#include "linear.h"
#include <iostream>
#include <cmath>

//Function Definitions MyVec3
using namespace std;
    MyVec3::MyVec3(float x, float y, float z){
        n[0] = x;
        n[1] = y;
        n[2] = z;
    }
   
    MyVec3 MyVec3::operator +(const MyVec3& c){
        int i;
        MyVec3 result = MyVec3();
        for(int i = 0; i < 3; i++){
            result.n[i] = this->n[i] + c.n[i];
        }
        return result;
    }
   
    MyVec3 MyVec3::operator -(const MyVec3& c){
        int i;
        MyVec3 result = MyVec3();
        for(int i = 0; i < 3; i++){
            result.n[i] = this->n[i] - c.n[i];
        }
        return result;
    }
   
    MyVec3 MyVec3::operator *(const MyVec3& c){
        MyVec3 result = MyVec3();
        result.n[0] = (this->n[1] * c.n[2]) - (c.n[1] * this->n[2]);
        result.n[1] = (-1 * (this->n[0] * c.n[2])) + (c.n[0] * this->n[2]);
        result.n[2] = (this->n[0] * c.n[1]) - (c.n[0] * this->n[1]);
        return result;
    }
   
    MyVec3 MyVec3::operator +(int& c){
        int i;
        MyVec3 result = MyVec3();
        for(int i = 0; i < 3; i++){
            result.n[i] = this->n[i] + c;
        }
        return result;
    }
   
    MyVec3 MyVec3::operator -(int& c){
        int i;
        MyVec3 result = MyVec3();
        for(int i = 0; i < 3; i++){
            result.n[i] = this->n[i] - c;
        }
        return result;
    }
   
    MyVec3 MyVec3::operator *(int& c){
        int i;
        MyVec3 result = MyVec3();
        for(int i = 0; i < 3; i++){
            result.n[i] = this->n[i] * c;
        }
        return result;
    }
   
    MyVec3 MyVec3::operator /(int& c){
        int i;
        MyVec3 result = MyVec3();
        for(int i = 0; i < 3; i++){
            result.n[i] = this->n[i] / c;
        }
        return result;
    }
   
    MyVec3 MyVec3::operator +(float& c){
        int i;
        MyVec3 result = MyVec3();
        for(int i = 0; i < 3; i++){
            result.n[i] = this->n[i] + c;
        }
        return result;
    }
   
    MyVec3 MyVec3::operator -(float& c){
        int i;
        MyVec3 result = MyVec3();
        for(int i = 0; i < 3; i++){
            result.n[i] = this->n[i] - c;
        }
        return result;
    }
   
    MyVec3 MyVec3::operator *(float& c){
        int i;
        MyVec3 result = MyVec3();
        for(int i = 0; i < 3; i++){
            result.n[i] = this->n[i] * c;
        }
        return result;
    }
   
    MyVec3 MyVec3::operator /(float& c){
        int i;
        MyVec3 result = MyVec3();
        for(int i = 0; i < 3; i++){
            result.n[i] = this->n[i] / c;
        }
        return result;
    }
   
    float MyVec3::dot(MyVec3 c){
        float result = 0;
        int i;
        for(i = 0; i < 3; i++){
            result += this->n[i] * c.n[i];
        }
        return result;
    }
    
    float MyVec3::getMagnitude()
    {
        return sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
    }
    
    MyVec3 distTwoPoints(MyVec3 a, MyVec3 b)
    {
        float dim1= (float)abs(a.n[0]-b.n[0]);
        float dim2= (float)abs(a.n[0]-b.n[0]);
        float dim3= (float)abs(a.n[0]-b.n[0]);
        return MyVec3(dim1,dim2,dim3);
    }
    
   
    MyMat3 MyVec3::makeSkew(){
        MyMat3 result = MyMat3(0,0,0,0,0,0,0,0,0);
        result.n[0] = 0;
        result.n[1] = this->n[2] * -1;
        result.n[2] = this->n[1];
        result.n[3] = this->n[2];
        result.n[4] = 0;
        result.n[5] = this->n[0] * -1;
        result.n[6] = this->n[1] * -1;
        result.n[7] = this->n[0];
        result.n[8] = 0;
        return result;        
    }
   
//Function Definitions MyVec4
    MyVec4::MyVec4(float x, float y, float z){
        n[0] = x;
        n[1] = y;
        n[2] = z;
    }
   
    MyVec4 MyVec4::operator +(const MyVec4& c){
        int i;
        MyVec4 result = MyVec4();
        for(int i = 0; i < 4; i++){
            result.n[i] = this->n[i] + c.n[i];
        }
        return result;
    }
   
    MyVec4 MyVec4::operator -(const MyVec4& c){
        int i;
        MyVec4 result = MyVec4();
        for(int i = 0; i < 4; i++){
            result.n[i] = this->n[i] - c.n[i];
        }
        return result;
    }
   
    MyVec4 MyVec4::operator +(int& c){
        int i;
        MyVec4 result = MyVec4();
        for(int i = 0; i < 4; i++){
            result.n[i] = this->n[i] + c;
        }
        return result;
    }
   
    MyVec4 MyVec4::operator -(int& c){
        int i;
        MyVec4 result = MyVec4();
        for(int i = 0; i < 4; i++){
            result.n[i] = this->n[i] - c;
        }
        return result;
    }
   
    MyVec4 MyVec4::operator *(int& c){
        int i;
        MyVec4 result = MyVec4();
        for(int i = 0; i < 4; i++){
            result.n[i] = this->n[i] * c;
        }
        return result;
    }
   
    MyVec4 MyVec4::operator /(int& c){
        int i;
        MyVec4 result = MyVec4();
        for(int i = 0; i < 4; i++){
            result.n[i] = this->n[i] / c;
        }
        return result;
    }
   
    MyVec4 MyVec4::operator +(float& c){
        int i;
        MyVec4 result = MyVec4();
        for(int i = 0; i < 4; i++){
            result.n[i] = this->n[i] + c;
        }
        return result;
    }
   
    MyVec4 MyVec4::operator -(float& c){
        int i;
        MyVec4 result = MyVec4();
        for(int i = 0; i < 4; i++){
            result.n[i] = this->n[i] - c;
        }
        return result;
    }
   
    MyVec4 MyVec4::operator *(float& c){
        int i;
        MyVec4 result = MyVec4();
        for(int i = 0; i < 4; i++){
            result.n[i] = this->n[i] * c;
        }
        return result;
    }
   
    MyVec4 MyVec4::operator /(float& c){
        int i;
        MyVec4 result = MyVec4();
        for(int i = 0; i < 4; i++){
            result.n[i] = this->n[i] / c;
        }
        return result;
    }
   
    //Function Def MyMat3
   
    MyMat3::MyMat3(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz){
        n[0] = xx;
        n[1] = xy;
        n[2] = xz;
        n[3] = yx;
        n[4] = yy;
        n[5] = yz;
        n[6] = zx;
        n[7] = zy;
        n[8] = zz;
    }
   
    MyMat3 MyMat3::operator +(const MyMat3& c){
        int i;
        MyMat3 result = MyMat3();
        for(i = 0; i < 9; i++){
            result.n[i] = this->n[i] + c.n[i];
        }
        return result;
    }
   
    MyMat3 MyMat3::operator -(const MyMat3& c){
        int i;
        MyMat3 result = MyMat3();
        for(i = 0; i < 9; i++){
            result.n[i] = this->n[i] - c.n[i];
        }
        return result;
    }
   
    MyMat3 MyMat3::operator *(const MyMat3& c){
        MyMat3 result = MyMat3();
        result.n[0] = MyVec3(this->n[0],this->n[1],this->n[2]).dot(MyVec3(c.n[0],c.n[3],c.n[6]));
        result.n[1] = MyVec3(this->n[0],this->n[1],this->n[2]).dot(MyVec3(c.n[1],c.n[4],c.n[7]));
        result.n[2] = MyVec3(this->n[0],this->n[1],this->n[2]).dot(MyVec3(c.n[2],c.n[5],c.n[9]));
        result.n[3] = MyVec3(this->n[3],this->n[4],this->n[5]).dot(MyVec3(c.n[0],c.n[3],c.n[6]));
        result.n[4] = MyVec3(this->n[3],this->n[4],this->n[5]).dot(MyVec3(c.n[1],c.n[4],c.n[7]));
        result.n[5] = MyVec3(this->n[3],this->n[4],this->n[6]).dot(MyVec3(c.n[2],c.n[5],c.n[9]));
        result.n[6] = MyVec3(this->n[6],this->n[7],this->n[8]).dot(MyVec3(c.n[0],c.n[3],c.n[6]));
        result.n[7] = MyVec3(this->n[6],this->n[7],this->n[8]).dot(MyVec3(c.n[1],c.n[4],c.n[7]));
        result.n[8] = MyVec3(this->n[6],this->n[7],this->n[8]).dot(MyVec3(c.n[2],c.n[5],c.n[9]));
        return result;
    }
   
    MyVec3 MyMat3::operator *(const MyVec3& c){
        MyVec3 result = MyVec3();
        result.n[0] = MyVec3(this->n[0],this->n[1],this->n[2]).dot(c);
        result.n[1] = MyVec3(this->n[3],this->n[4],this->n[5]).dot(c);
        result.n[2] = MyVec3(this->n[6],this->n[7],this->n[8]).dot(c);
        return result;
    }
   
    MyMat3 MyMat3::operator +(float& c){
        int i;
        MyMat3 result = MyMat3();
        for(int i = 0; i < 9; i++){
            result.n[i] = this->n[i] + c;
        }
        return result;        
    }
   
    MyMat3 MyMat3::operator -(float& c){
        int i;
        MyMat3 result = MyMat3();
        for(int i = 0; i < 9; i++){
            result.n[i] = this->n[i] - c;
        }
        return result;        
    }
   
    MyMat3 MyMat3::operator *(float& c){
        int i;
        MyMat3 result = MyMat3();
        for(int i = 0; i < 9; i++){
            result.n[i] = this->n[i] * c;
        }
        return result;        
    }
   
    MyMat3 MyMat3::operator /(float& c){
        int i;
        MyMat3 result = MyMat3();
        for(int i = 0; i < 9; i++){
            result.n[i] = this->n[i] / c;
        }
        return result;        
    }
   
   /* MyVec3 MyMat3::multVec(MyVec3 M)
    {
           this-diagInverse()
           MyVec3 result= MyVec3(0,0,0);
           result.n[0]=this->n[0]*M.n[0]+this.n[1]*M.n[1]+this.n[2]*M.n[2];
           result.n[1]=this.n[3]*M.n[0]+this.n[4]*M.n[1]+this.n[5]*M.n[2];
           result.n[2]=this.n[6]*M.n[0]+this.n[7]*M.n[1]+this.n[8]*M.n[2];
           return result;
    }*/
                         
   
    MyMat3 MyMat3::diagInverse(){
        //This method is designed to return the inverse of the Inertia Tensor matrix which is diagonal.
        //It should NOT be used for anything else;
        MyMat3 result = MyMat3();
        result.n[0] = 1.0 / this->n[0];
        result.n[1] = 0;
        result.n[2] = 0;
        result.n[3] = 0;
        result.n[4] = 1.0 / this->n[4];
        result.n[5] = 0;
        result.n[6] = 0;
        result.n[7] = 0;
        result.n[8] = 1.0 / this->n[8];
    }
   
    void MyMat3::printOut()
    {
         cout<<n[0]<<n[1]<<n[2]<<n[3]<<n[4]<<n[5]<<n[6]<<n[7]<<n[8]<<endl;
    }

